#include <vector>
#include "qgcmmath.h"
#include "misclib.h"

// Note that this function is not terribly efficient.
// Use the BLITZ++ Array class for better efficiency here.

/* ------------------------------------------------------------------------- */
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
/*Linear equation solution by Gauss-Jordan elimination, equation (2.1.1) above. a[0..n-1][0..n-1]
  is the input matrix. b[0..n-1][0..m-1] is input containing the m-1 right-hand side vectors. On
  output, a is replaced by its matrix inverse, and b is replaced by the corresponding set of solution
  vectors.*/
bool invertmatrix(vector< vector<double> > & a){ // n rows and n columns
	const int n = static_cast<int>( a.size() );
	const int m = n + 1;
	// check that the matrix is a block matrix
	vector< vector<double> > b;
	for(int i = 0; i < n; i++){
		b.push_back( vector<double>( m, 0.0 ) );
		rassert( a[i].size() == static_cast<size_t>(n) ); // otherwise this is not a block matrix
		for(int j = 0; j < n; j++){
			b[i][j] = a[i][j];
		}
		b[i][m-1] = 1.0;
	}
	//int *indxc,*indxr,*ipiv;
	vector<int> indxc(n,0);
	vector<int> indxr(n,0);
	vector<int> ipiv(n,0);
	int i,icol,irow,j,k,l,ll;
	double big;
	double dum,pivinv,temp;
	//indxc = new int[n]; // The integer arrays ipiv, indxr, and indxc are used for bookkeeping on the pivotiindxr=ivector(1,n); ng.
	//ipiv  = new int[n];
	//indxr = new int[n];
	for (j=0;j<n;j++) ipiv[j]=-1;
	for (j=0;j<n;j++) indxc[j]=-1;
	for (j=0;j<n;j++) indxr[j]=-1;
	for (i=0;i<n;i++) { //This is the main loop over the columns to be
		big=(0.0); //reduced.
		for (j=0;j<n;j++){ //This is the outer loop of the search for a pivot
			if (ipiv[j] != 0){ //element.
				for (k=0;k<n;k++) {
					if (ipiv[k] == -1) {
						if (fabs(a[j][k]) >= big) { // 
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
						}
					}else if(ipiv[k] > 0){
						fprintf(stderr, "Singular Matrix provided to function '%s'.\n", __PRETTY_FUNCTION__);
						return false;
					}
				}
			}
		}
		++(ipiv[icol]);
		/*We now have the pivot element, so we interchange rows, if needed, to put the pivot
		  element on the diagonal. The columns are not physically interchanged, only relabeled:
		  indxc[i], the column of the ith pivot element, is the ith column that is reduced, while
		  indxr[i] is the row in which that pivot element was originally located. If indxr[i] .=
		  indxc[i] there is an implied column interchange. With this form of bookkeeping, the
		  solution b<92>s will end up in the correct order, and the inverse matrix will be scrambled
		  by columns.*/
		if (irow != icol) {
			for (l=0;l<n;l++) { SWAP(a[irow][l],a[icol][l]) }
			for (l=0;l<m;l++) { SWAP(b[irow][l],b[icol][l]) }
		}
		indxr[i]=irow; //We are now ready to divide the pivot row by the
		indxc[i]=icol; //pivot element, located at irow and icol.
		if ((a[icol][icol]==(0.0))){
			fprintf(stderr, "Singular Matrix provided to function '%s'.\n", __PRETTY_FUNCTION__);
			return false;
		}
		//double pivelement = (a[icol][icol]); /// used in debugger
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=(1.0);
		for (l=0;l<n;l++) {
			a[icol][l] *= pivinv;
		}
		for (l=0;l<m;l++) {
			b[icol][l] *= pivinv;
		}
		for (ll=0;ll<n;ll++){ //Next, we reduce the rows...
			if (ll != icol) { //...except for the pivot one, of course.
				dum=a[ll][icol];
				a[ll][icol]=(0.0);
				for (l=0;l<n;l++) {
					double pval = a[icol][l]*dum;
					a[ll][l] = a[ll][l] - pval;
				}
				for (l=0;l<m;l++) {
					double pval = b[icol][l]*dum;
					b[ll][l] = b[ll][l] - pval;
				}
			}
		}
	}
	/*This is the end of the main loop over columns of the reduction. It only remains to unscramble
	  the solution in view of the column interchanges. We do this by interchanging pairs of
	  columns in the reverse order that the permutation was built up. */
	for (l=(n-1);l>=0;l--) {
		if (indxr[l] != indxc[l]){
			for (k=0;k<n;k++){
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
			}
		}
	} // And we are done.
	//delete [] ipiv;
	//delete [] indxr;
	//delete [] indxc;
	return true;
}
#undef SWAP



